#ifndef SERVO_H
#define SERVO_H

#include <Servo.h>

int step_angle = 5; 
int cur_angle = 0; 


// servo action code
const int SERVO_CLOCKWISE = 1;
const int SERVO_ANTI_CLOCKWISE = 2;
const int SERVO_90 = 3;
int cur_servo_cmd=SERVO_90;

const int SERVO_PIN=11;

Servo ultrasonic_servo;

void turning_90() 
{
   cur_angle=90;
   ultrasonic_servo.write(cur_angle); 
}

void turning_clockwise() {
  cur_angle = ultrasonic_servo.read(); 
  for (int i = 0; i < step_angle; i++) {  
    cur_angle -= 1;
    if (cur_angle <=0 ) { cur_angle =0; };
    ultrasonic_servo.write(cur_angle);  
    delay(15);        
 }
}

void turning_anti_clockwise() {
  cur_angle = ultrasonic_servo.read(); 
  for (int i = 0; i < step_angle; i++) {  
    cur_angle += 1;
    if (cur_angle >= 180 ) { cur_angle = 80; };
    ultrasonic_servo.write(cur_angle);  
    delay(15);        
 }
}

void servo_action(int servo_cmd) {
  switch (servo_cmd) {
    case SERVO_CLOCKWISE:
       turning_clockwise();
      break;
    case SERVO_ANTI_CLOCKWISE:
      turning_anti_clockwise();
      break;
   case SERVO_90:
      turning_90();
      break;
    default:
      break;
  }  // switch
};

void setup_servo() {
  Serial.begin(9600);
  ultrasonic_servo.attach(SERVO_PIN);
  cur_servo_cmd=SERVO_90;
  servo_action(cur_servo_cmd);
}

void loop_servo() {
} 

#endif /* SERVO_H */